#include "speed_cnrt.h"

//! Cointains data for timer interrupt.
SpeedRampData srd_x = {0};
SpeedRampData srd_y = {0};


ushort speed_cntr_max_index(ushort speed)
{
    ushort max_index = (ulong)speed * speed * ( 1 / (2 * ALPHA * ACCEL * 100));
    if (max_index > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        max_index = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;
    return max_index;
}

void speed_cntr_Move(SpeedRampData *srd, long step, ushort speed)
{
    if (step == 0)
        return;

    // Set direction from sign on step value.
    if(step < 0)
    {
        srd->dir = CCW;
        srd->step_offset = -1;
        step = -step;
    }
    else
    {
        srd->dir = CW;
        srd->step_offset = 1;
    }

    // 最大速度对应的速度表索引
    // n = speed^2 / (2*alpha*accel)
    srd->max_index = (ulong)speed * speed * ( 1 / (2 * ALPHA * ACCEL*100));
    if (srd->max_index > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->max_index = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;
    srd->max_ideal_index = srd->max_index;
    srd->max_ref_index = srd->max_index;

    srd->speed_index = srd->max_index;
    //srd->speed_index = MIN_SPEED_INDEX;
    srd->step_left = step;
    srd->run_state = MOTION_STATE_RUN;

    srd->hit_home = 0;
    srd->running = 1;
    srd->need_cleanup = 0;
    srd->ref_changed = 0;
    //srd->start_time = SystemTick();
    srd->step_count = 0;
    srd->step_correction = 0;
    srd->init();
}
int i = 0;
void speed_cntr_update(SpeedRampData *srd)
{
    if (!srd->running)
        return;

#if MOVE_METHOD != MOVE_METHOD_INTERP
    #ifdef PULSE_DUTY_CYCLES_50
        srd->start_timer(FREQ_TABLE(srd->speed_index) >> 1);
    #else
        srd->start_timer(FREQ_TABLE(srd->speed_index));
    #endif// PULSE_DUTY_CYCLES_50
#endif// MOVE_METHOD

    if (srd->run_state == MOTION_STATE_STOP)
    {
#if MOVE_METHOD != MOVE_METHOD_INTERP
        srd->stop_timer();
#endif
        srd->need_cleanup = 1;
    }
    else
    {
#if !defined(PULSE_DUTY_CYCLES_50) || (MOVE_METHOD == MOVE_METHOD_INTERP)
        // 输出高电平
        srd->step(1);
#else
        srd->output = !srd->output;
        srd->step(srd->output);
        if (srd->output)
            return;
#endif
        i++;
        i %= 32;
        if(i == 31)
        {
            // 更新已经运行的步数和剩余步数
            srd->step_count++;
            srd->step_left--;

            // 走一步, 调用更新函数, 用以检查传感器状态, 更新剩余步数等
            srd->update();

            if (srd->step_left <= 0)
            {
                srd->run_state = MOTION_STATE_STOP;
    #if defined(PULSE_DUTY_CYCLES_50) && MOVE_METHOD != MOVE_METHOD_INTERP
                srd->stop_timer();
                srd->need_cleanup = 1;
    #endif
            }
            else
            {
                // 如果参考坐标系改变, 更新最大输出速度
                if (srd->ref_changed)
                {
                    srd->max_index = min(srd->max_ideal_index, srd->max_ref_index*2);
                    srd->ref_changed = 0;
                }

                // 更新下一步速度
                if (srd->speed_index > srd->step_left)
                    srd->speed_index -= srd->speed_index / (ushort)srd->step_left;
                else if (srd->speed_index > srd->max_index)
                    srd->speed_index--;
                else if (srd->speed_index < srd->max_index)
                    srd->speed_index++;
                
            }
        }

#if !defined(PULSE_DUTY_CYCLES_50) || (MOVE_METHOD == MOVE_METHOD_INTERP)
        // 输出低电平
        srd->step(0);
#endif
    }
}
